#include "mc.h"

const unsigned int ADC_MASK[8]	=   {0x0000,0x0002,0x0001,0x0004,0x0002,0x0001,0x0004,0x0000};
const unsigned int ADC_XOR[8]	=   {0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0x0000,0xFFFF,0x0000};
const unsigned int PWM_STATE[]	=	{0x0000,0x2001,0x2004,0x0204,0x0210,0x0810,0x0801,0x0000}; //M3/M1,
const unsigned char ADC_BEMF_FILTER[64]=
									{0x00,0x02,0x04,0x06,0x08,0x0A,0x0C,0x0E,0x10,0x12,0x14,0x16,0x18,0x1A,0x1C,0x1E,
									 0x20,0x22,0x24,0x26,0x28,0x2A,0x2C,0x2E,0x01,0x01,0x01,0x36,0x01,0x3A,0x3C,0x3E,
									 0x00,0x02,0x04,0x06,0x08,0x0A,0x0C,0x0E,0x01,0x01,0x01,0x16,0x01,0x1A,0x1C,0x1E,
									 0x01,0x01,0x01,0x26,0x01,0x2A,0x2C,0x2E,0x01,0x01,0x01,0x36,0x01,0x3A,0x3C,0x3E};

struct {
			unsigned RunMotor : 	1;
			unsigned RampUpBEMF :	1;
			unsigned unused :		14;
		}	Flags;

struct {

   			unsigned PhaseAOutput : 	1;
			unsigned PhaseBOutput : 	1;
			unsigned PhaseCOutput : 	1;
			unsigned Commutation  : 	1;
			unsigned unused 	  :		12;

		}	Comparator;

unsigned int  PWMticks;
unsigned char CommState;
unsigned char ADCCommState;
unsigned char bemf_filter;
unsigned char ADC_bemf_filter;
unsigned char comm_done;

unsigned int u16BackEMFabc;
unsigned int u16MotorNeutralVoltage;
unsigned int u16MotorPhaseA;
unsigned int u16MotorPhaseB;
unsigned int u16MotorPhaseC;
unsigned int u16ComparatorOutputs;

unsigned int u16CommutationStatus;


unsigned char u8PoleCounter;

int timer3value;
int timer3avg;
int DesiredSpeed;
int ActualSpeed;
int SpeedError;
int SpeedIntegral;
int PILoopControllerOutput;
int u16DesiredPWMDutyCycle;
unsigned int Kps;
unsigned int Kis;
unsigned int u16CurrentPWMDutyCycle;

void InitMCClock(void)
{
	OSCCONbits.NOSC = 1;		//FRC with PLL
	OSCCONbits.CLKLOCK = 1;		//CLOCK switching is disabled
	OSCCONbits.LPOSCEN = 0;		//Disable Second OSC

	// Configure Oscillator to operate the device at 29491200Hz
	// Fosc= Fin*M/(N1*N2), Fcy=Fosc/2
	// Fosc= 7.3728MHz*32/(2*2)= 58.9824Mhz for FRC input clock
	CLKDIVbits.DOZE = 0;
	CLKDIVbits.DOZEN = 0;
	CLKDIVbits.FRCDIV = 0;		
	CLKDIVbits.PLLPOST = 0;		// N1=2
	CLKDIVbits.PLLPRE = 0;		// N2=2
	PLLFBD = 30;				// M=32

	while(OSCCONbits.LOCK != 1) {};		// Wait for PLL to lock
}

void InitMCStuff(void)
{
	/*PWMs are outputs*/
	LATBbits.LATB9 = 1;		//PWM_EN, FIRE_EN in the MC Pictail Duaghter Board //TODO: brauchen wir das?
	TRISBbits.TRISB9 = 0;	
	
	LATBbits.LATB10 = 0;	//PWM1H3
	TRISBbits.TRISB10 = 0;
	LATBbits.LATB11 = 0;	//PWM1L3
	TRISBbits.TRISB11 = 0;
	LATBbits.LATB12 = 0;	//PWM1H2
	TRISBbits.TRISB12 = 0;
	LATBbits.LATB13 = 0;	//PWM1L2
	TRISBbits.TRISB13 = 0;	
	LATBbits.LATB14 = 0;	//PWM1H1
	TRISBbits.TRISB14 = 0;	
	LATBbits.LATB15 = 0;	//PWM1L1
	TRISBbits.TRISB15 = 0;	

	
	Comparator.Commutation = 0;	

	/*Push Buttons ports*/
	LATBbits.LATB5 = 0;
	TRISBbits.TRISB5 = 1;
	LATBbits.LATB6 = 0;
	TRISBbits.TRISB6 = 1;

	INTCON1bits.NSTDIS = 0;		    // Enabling nested interrupts	//TODO: brauchen wir das?
	InitMCPWM();
	InitADC10();
	InitTMR3();
	LATBbits.LATB9 = 0;	//PWM_EN, FIRE_EN in the MC Pictail Duaghter Board //TODO: brauchen wir das?
	
	u8PoleCounter = 1;
	u16DesiredPWMDutyCycle = 0;
	DesiredSpeed = 0;
	ActualSpeed = 0;
	SpeedError = 0;
	SpeedIntegral = 0;
	Kps = 3000;
	Kis = 2;
	PILoopControllerOutput = 0;
	timer3value = 0;
	timer3avg = 0;

}

/******************************************************************************
* Function:     main(void)
*
* Output:		None
*
* Overview:		Main funtion used to init the ADC, PWM and TIMER3 modules.
*				It also inits the global variables used in the interrupts and
*				PI controller.
*				The main task excuted here is to start and stop the motor
*				as well as setting the	ramp-up initial parameters to
*				spin the motor
*
* Note:			None
*******************************************************************************/
void mcMain(void)
{
	for(;;)
		{	
		while (!S3);					// wait for S3 button to be hit
		while (S3)						// wait till button is released
			DelayNmSec(20);

		u16CurrentPWMDutyCycle = 1;
		u16DesiredPWMDutyCycle = 0;
		DesiredSpeed = 0;
		ActualSpeed = 0;
		SpeedError = 0;
		SpeedIntegral = 0;
		PILoopControllerOutput = 0;
		timer3value = 0;
		timer3avg = 0;
		

		PWM1CON1 = 0x0777;				// enable PWM outputs
		DelayNmSec(1);
		Flags.RampUpBEMF =1;
		Flags.RunMotor = 1;				// indicationg the run motor condition



		while (Flags.RunMotor)			// while motor is running
			{


			while(Flags.RampUpBEMF)
				{
				if(u16CurrentPWMDutyCycle < MIN_DUTY_CYCLE) // 1469 = 100% duty cycle
					{
	    		   	u16CurrentPWMDutyCycle++;
					DelayNmSec(1);
					}else{
						//u16CurrentPWMDutyCycle = MIN_DUTY_CYCLE;
						//u16DesiredPWMDutyCycle = MIN_DUTY_CYCLE;
						Flags.RampUpBEMF = 0;
						T3CONbits.TON = 1;
						}
				}


			if (S3)						// if S5 is pressed
				{
				
				PWM1CON1 = 0x0700;		// disable PWM outputs
  				P1OVDCON = 0x0000;		// overide PWM low.
				Flags.RampUpBEMF = 0;
				Flags.RunMotor = 0;		// reset run flag
				u16CurrentPWMDutyCycle = 1;
				
				while (S3)				// wait for key release
					DelayNmSec(20);
				}


			}//end of motor running loop
		}//end of infinite loop
}//end of main function


/******************************************************************************
* Function:     _ADCInterrupt(void) 
*
* Output:		None
*
* Overview:		ADC interrupt used to measure the BEMF signals, recontruct
* 				the Motor Virtual Neutral Point and compare the BEMF signals
*				agains the neutral point reference to detect the zero-crossing 
*				event
*
* Note:			None
*******************************************************************************/
void __attribute__((__interrupt__,auto_psv)) _ADC1Interrupt(void)
{

	DesiredSpeed = ADC1BUF0;
	u16MotorPhaseA = ADC1BUF1;
	u16MotorPhaseB = ADC1BUF2;
	u16MotorPhaseC = ADC1BUF3;
	u16MotorNeutralVoltage = (u16MotorPhaseA + u16MotorPhaseB + u16MotorPhaseC)/3;
	AD1CON1bits.DONE = 0;
	IFS0bits.AD1IF = 0;

	/********************* ADC SAMPLING & BMEF signals comparison ****************/
	if(u16MotorPhaseA > u16MotorNeutralVoltage)
		{
		Comparator.PhaseAOutput = 1;
		} 
		else {
			Comparator.PhaseAOutput = 0;
			}
	if(u16MotorPhaseB > u16MotorNeutralVoltage)
		{
		Comparator.PhaseBOutput = 1;
		} 
		else {
			Comparator.PhaseBOutput = 0;
			}
	if(u16MotorPhaseC > u16MotorNeutralVoltage)
		{
		Comparator.PhaseCOutput = 1;
		} 
		else {
			Comparator.PhaseCOutput = 0;
			}

	u16ComparatorOutputs = (Comparator.PhaseCOutput<<2) & 0x0007;
	u16ComparatorOutputs |= (Comparator.PhaseBOutput<<1);
	u16ComparatorOutputs |= Comparator.PhaseAOutput;	

}


/******************************************************************************
* Function:     _PWMInterrupt(void)
*
* Output:		None
*
* Overview:		PWM relod interrupt used to filter the BEMF signals using the
*				majority detection filter to detect a valid xero-crossing event
*				if a valid zero-cressignevent was detected then Commutate.
*				This funtion also includes the start-upt sequence for detecting
*				the intial rotor position
*
* Note:			None
*******************************************************************************/
void __attribute__((__interrupt__,auto_psv)) _MPWM1Interrupt (void)
{
	
	if((u16ComparatorOutputs^ADC_XOR[ADCCommState])& ADC_MASK[ADCCommState])
		{
		ADC_bemf_filter|=0x01;
		}

	if(u16CurrentPWMDutyCycle>160)
		P1SECMPbits.SEVTCMP = u16CurrentPWMDutyCycle>>1;
	else if(u16CurrentPWMDutyCycle>76)
		//P1SECMPbits.SEVTCMP =48;
		P1SECMPbits.SEVTCMP = u16CurrentPWMDutyCycle>>3;
	else
		P1SECMPbits.SEVTCMP = 0;


	ADC_bemf_filter = ADC_BEMF_FILTER[ADC_bemf_filter];
	if (ADC_bemf_filter&0b00000001)
		{
 		Commutate();
		Comparator.Commutation = ~ Comparator.Commutation;
		u16CommutationStatus = 	Comparator.Commutation;
		}
	//Ramp-up period to detect the rotor position
	if(Flags.RampUpBEMF)
    	P1DC1=P1DC2=P1DC3=u16CurrentPWMDutyCycle;
	if (++PWMticks>MAX_PWMticks)
		Commutate();
	
	IFS3bits.PWM1IF = 0;

}

/******************************************************************************
* Function:     Commutate(void)
*
* Output:		None
*
* Overview:		Here is where the motor commutation occurs, this funtion also
*				computates the rotor speed using the TIMER3
*
* Note:			None
*******************************************************************************/
void Commutate(void)
{
	// The code below is uses TMR3 to calculate the speed of the rotor
	if (ADCCommState == INDEX)				// has the same position been sensed?
		if (u8PoleCounter++ >= POLEPAIRS)  	// has one mech rev elasped?
		{									// if yes then read timer 3
		T3CONbits.TON = 0;	// Stop TIMER3
		timer3value = TMR3;
		TMR3 = 0;
		timer3avg = ((timer3avg + timer3value) >> 1);

		if(!Flags.RampUpBEMF)
			SpeedPILoopController();
	
		u8PoleCounter = 1;
		T3CONbits.TON = 1;	// Start TIMER3

		} 

	//Commutation
	ADC_bemf_filter=0;
	if (++ADCCommState>6)
		ADCCommState=1;
	P1OVDCON=PWM_STATE[ADCCommState];

	PWMticks=0;

}

/******************************************************************************
* Function:     SpeedPILoopController(void)
*
* Output:		None
*
* Overview:		When the CLOSELOOPMODE macro is defined the compiler adds
*				the PI controller for the speed, if CLOSELOOPMODE is not defined
*				then the motor operates in open loop mode. The Kp and Ki 
*				paratemers were determined using the HURST MOTOR shipped with 
*				the MCLV board. This values should be modified according to 
*				the motor specs.
*
* Note:			None
*******************************************************************************/
/******************************************************************************/
/***************          CLOSE LOOP MODE              ************************/
/******************************************************************************/
#ifdef CLOSELOOPMODE
void SpeedPILoopController(void)
{

    //For the HURST motor the Timer3Avg values
	//TIMER3 counts = 9562 AT 7.5% dutycycle (P1DC1 = 109) 
	//TIMER3 counts = 714  AT 100% dutycycle (P1DC1 = 1469)
	//RPSelectrical = 19-261Hz 
 	//RPMelctrical = 1179-15669RPM 

	//Normalizing TIMER3 counts to electrical RPM
	ActualSpeed = (SPEEDMULT/timer3avg)*60; 	
	//ADC POT RANGE 0-1024, 1024*16 = MAXSPEED in electrical RPM 									 	
	DesiredSpeed = DesiredSpeed<<4;
	//Calculating Error
	SpeedError = DesiredSpeed - ActualSpeed;
	SpeedIntegral += SpeedError;
	//Dividing PI output by PI controller gain (2exp14)
	PILoopControllerOutput = (((long)Kps*(long)SpeedError + (long)Kis*(long)SpeedIntegral) >> 14); 
	//Divided by 11 to normalize the PI output to PWM PDCx values 
	u16DesiredPWMDutyCycle = u16DesiredPWMDutyCycle + (PILoopControllerOutput/11); 	
	// Max and Min PWM duty cycle limits
	if (u16DesiredPWMDutyCycle < MIN_DUTY_CYCLE)
		{
		u16DesiredPWMDutyCycle = MIN_DUTY_CYCLE;
		SpeedIntegral = 0;
		}
	if (u16DesiredPWMDutyCycle > MAX_DUTY_CYCLE)
		{
		u16DesiredPWMDutyCycle = MAX_DUTY_CYCLE;
		SpeedIntegral = 0;
		} 

	P1DC1 = u16DesiredPWMDutyCycle;
	P1DC2 = u16DesiredPWMDutyCycle;
	P1DC3 = u16DesiredPWMDutyCycle;
	u16CurrentPWMDutyCycle = u16DesiredPWMDutyCycle;	

}

/******************************************************************************/
/***************          OPEN LOOP MODE               ************************/
/******************************************************************************/
#else 
void SpeedPILoopController(void)
{
	//PWM duty cycle = pot value *2
	u16DesiredPWMDutyCycle = DesiredSpeed<<1;
	u16CurrentPWMDutyCycle = u16DesiredPWMDutyCycle;

	if(u16CurrentPWMDutyCycle != u16DesiredPWMDutyCycle)
		{
		if(u16CurrentPWMDutyCycle < u16DesiredPWMDutyCycle)
			u16CurrentPWMDutyCycle++;		
		if(u16CurrentPWMDutyCycle > u16DesiredPWMDutyCycle)
			u16CurrentPWMDutyCycle--;
		}
		
	if (u16CurrentPWMDutyCycle < MIN_DUTY_CYCLE)
		u16CurrentPWMDutyCycle = MIN_DUTY_CYCLE;
	if (u16CurrentPWMDutyCycle > MAX_DUTY_CYCLE)
		u16CurrentPWMDutyCycle = MAX_DUTY_CYCLE;

	P1DC1 = u16CurrentPWMDutyCycle;
	P1DC2 = u16CurrentPWMDutyCycle;
	P1DC3 = u16CurrentPWMDutyCycle;

} 
#endif

/******************************************************************************
* Function:     InitADC10(void)
*
* Output:		None
*
* Overview:		Initializes the ADC module to operate in sumoultaneous mode
*				sampling terminals AN0,AN1, AN2, AN3 using MUX A. The ADC channels are
*				assigned as follows in the MCLV board
*				CH0->AN3  (POT)
*				CH1->AN0 (HALLA, PHASE A)
*				CH2->AN1 (HALLB, PHASE B)
*				CH3->AN2 (HALLC, PHASE C)
* 				ADC is sync with the PWM. ADC is conversion is triggered
*				everytim a PWM relod event occurs. Tadc = 84.75 nSec.
*				ADC resulting samples are formatted as unsigned 10-bits 
*				left-justified 
*
* Note:			None
*******************************************************************************/
void InitADC10(void)
{

	AD1PCFGL = 0x0003;	//Port pin multiplexed with AN0-AN1 in Digital mode
						//Port pin multiplexed with AN2-AN5 in Analog mode



	AD1CON1 = 0x006C;   //ADC is off
						//Continue module operation in Idle mode
						//10-bit, 4-channel ADC operation
						//Data Output Format bits Integer (0000 00dd dddd dddd)
						//011 = Motor Control PWM interval ends sampling and starts conversion
						//Samples CH0, CH1, CH2, CH3 simultaneously when CHPS<1:0> = 1x
						//Sampling begins immediately after last conversion SAMP bit is auto-set.

	AD1CHS123 = 0x0001; //MUX B CH1, CH2, CH3 negative input is VREF-
						//MUX B CH1 positive input is AN0, CH2 positive input is AN1, CH3 positive input is AN2						
						//MUX A CH1, CH2, CH3 negative input is VREF-
						//MUX A CH1 positive input is AN3, CH2 positive input is AN4, CH3 positive input is AN5

	AD1CHS0 = 0x0602;   //MUX B Channel 0 negative input is VREF-
						//MUX B Channel 0 positive input is AN6
						//MUX A Channel 0 negative input is VREF-
						//MUX A Channel 0 positive input is AN2

	AD1CSSL = 0x0000;	//Skip all ANx channels for input scan


	AD1CON3 = 0x0002;	//ADC Clock derived from system clock
						//Autosample time time bits = 0 TAD sinjce PWM is controlling sampling time
						//TAD = 3*TCY, TAD = 101.7 nSec


	AD1CON2 = 0x0300;	//ADREF+ = AVDD ADREF- = AVSS
						//Do not scan inputs
						//1x = Converts CH0, CH1, CH2 and CH3
						//A/D is currently filling buffer 0x0-0x7
						//Interrupts at the completion of conversion for each sample/convert sequence
						//Always starts filling buffer from the beginning
						//Always uses channel input selects for Sample A

	AD1CON1bits.DONE = 0;
	IPC3bits.AD1IP = 5;	
	IFS0bits.AD1IF = 0;	
	IEC0bits.AD1IE = 1;
	AD1CON1bits.ADON = 1;

}


/******************************************************************************
* Function:     InitMCPWM(void)
*
* Output:		None
*
* Overview:		Initializes the PWM module to operate in center-aligned mode
*				at 20KHz. PWM terminals are configured in independent mode.
*				PWM time base is 67.8 nSec.
*				PDCx value range is 0-1464 for 0%-100% duty cycle 
*				ADC reload time is variable according to the PWM dutycycle
*
*
* Note:			None
*******************************************************************************/
void InitMCPWM(void)
{
	P1TPER = ((FCY/FPWM)/2 - 1);
							//FCY  29491200...FRC w/PLL x16
							//FPWM 20KHz PWM Freq
							// MAX_DUTY_CYCLE = 1469
							// 50% duty cycle = 734

	
	P1TCONbits.PTSIDL = 1;  // PWM time base halted in CPU IDLE mode
	P1TCONbits.PTOPS = 0;   // PWM time base 1:1 postscale
	P1TCONbits.PTCKPS = 0;  // PWM time base 1:1 prescale
	P1TCONbits.PTMOD = 2;	// Center Aligned with single interrupt mode per PWM period

	PWM1CON1 = 0x0700;		// disable PWMs
	P1OVDCON = 0x0000;		// allow control using OVD

	P1SECMPbits.SEVTDIR = 0; 	// trgger ADC when PWM counter is in upwards dir
								//....Tad=84.77, Tpwm=67.816
	P1SECMPbits.SEVTCMP = 0;	// generates a trigger event for the ADC
							// when PWM time base is counting upwards
							// just before reaching the PTPER value
							// causing a sampling in the middle of the 
							// pulse

	//kP1FLTACON = 0x008F;		// The PWM output pin is driven Inactive on 
							// an external fault input event
							// The Fault A input pin latches all control pins to 0
							// PWMxH4-1/PWMxL4-1 pin pair is controlled by 
							// Fault Input A


    PWM1CON2 = 0x0000;		// 1:1 postscale values
							// Updates to the active PxDCy registers 
							// are sync to the PWM time base
							// Output overrides via the PxOVDCON register occur
							// on the next TCY boundary
							// Updates from duty cycle and period buffer registers 
							// are enabled

	IPC14bits.PWM1IP = 4;	// PWM Interrupt Priority 4
	IFS3bits.PWM1IF=0;		// clear 
	IEC3bits.PWM1IE=1;


	P1TCONbits.PTEN = 1;
}

/******************************************************************************
* Function:     InitTMR3(void)
*
* Output:		None
*
* Overview:		Initializes the TIMER3 module to operate in free-running 
*				upcounting mode. The TIMER3 time base is Tcy/256 = 8.68uSec.
*				This timer is used to calculate the motor speed
*
* Note:			None
*******************************************************************************/
void InitTMR3(void)
{							// Tcy = 33.908 nSec
	TMR3 = 0;
	PR3 = 0xFFFF;
	T3CON = 0x0030;			// internal Tcy*256 clock = 8.68 uSec
}




/******************************************************************************
* Function:     DelayNmSec(unsigned int N)
*
* Output:		None
*
* Overview:		Delay funtion used for push buttons debounce loop and for the
*				motor start-up sequence
*
* Note:			None
*******************************************************************************/
void DelayNmSec(unsigned int N)
{
unsigned int j;
while(N--)
 	for(j=0;j < MILLISEC;j++);
}

